#########################################################################################
# 作者：hehung
# 联系：1398660197@qq.com
# 微信：hehung95
# 描述：该脚本主要用于使用键盘来标定舵机输出值，使用脚本上的“上下左右”四个按键来标定每一个舵机
#       输出值
#########################################################################################

import keyboard
import time
import uart_ctrl

# 必须运行在root权限下
class class_tuning():
    def __init__(self):
        self.g_steer = [7.5, 7.5, 7.5, 7.5, 7.5, 7.5]
        self.g_steer_flag = 0
        self.key_right_pressed = False
        self.key_left_pressed = False
        self.app_uart = uart_ctrl.class_uart()
    
    def send_command_to_servo(self):
        # data_str = 'START::0-%.2f:1-%.2f:2-%.2f:3-%.2f:4-%.2f:5-%.2f::END\r\n' % (self.g_steer[0], self.g_steer[1], self.g_steer[2], self.g_steer[3], self.g_steer[4], self.g_steer[5])
        # print ('Send Data: %s' % data_str.encode())
        # self.app_uart.send(data_str)
        self.app_uart.servo_ctrl(self.g_steer)

    # 注册按键松开函数
    def key_release_evt(self, key):
        self.key_right_pressed = False
        self.key_left_pressed = False
        # print (f'{key.name} is pressed')
        if (key.name == 'up' or key.name == 'down'):
            self.send_command_to_servo()

    def tuning_main(self):
        keyboard.on_release(self.key_release_evt) 
        while True:
            if keyboard.is_pressed('esc'):
                # 按下ESC退出
                break
            else:
                if keyboard.is_pressed('up'):
                    self.g_steer[self.g_steer_flag] = self.g_steer[self.g_steer_flag] + 0.1
                    if (self.g_steer[self.g_steer_flag] > 12.5):
                        self.g_steer[self.g_steer_flag] = 12.5
                elif keyboard.is_pressed('down'):
                    self.g_steer[self.g_steer_flag] = self.g_steer[self.g_steer_flag] - 0.1
                    if (self.g_steer[self.g_steer_flag] < 2.5):
                        self.g_steer[self.g_steer_flag] = 2.5

                if (self.key_right_pressed == False):
                    if keyboard.is_pressed('right'):
                        self.key_right_pressed = True
                        if self.key_right_pressed:
                            self.g_steer_flag = self.g_steer_flag + 1
                            if (self.g_steer_flag > 5):
                                self.g_steer_flag = 5

                if (self.key_left_pressed == False):
                    if keyboard.is_pressed('left'):
                        self.key_left_pressed = True
                        if self.key_left_pressed:
                            self.g_steer_flag = self.g_steer_flag - 1
                            if (self.g_steer_flag < 0):
                                self.g_steer_flag = 0
                # value = keyboard.read_key()
                # print ('Recv key: ', value)
                time.sleep(0.1)